
#include <winsock2.h>   // must include before windows.h or ws2tcpip.h
#include <windows.h>
#include <Ws2tcpip.h>
#include <tchar.h>
#include <math.h>
#include "NATUtils.h"
#include <float.h>

#include <winsock2.h>
#include <iphlpapi.h>
#include <stdio.h>
#include <stdlib.h>

#pragma comment(lib, "IPHLPAPI.lib")
#define MALLOC(x) HeapAlloc(GetProcessHeap(), 0, (x))
#define FREE(x) HeapFree(GetProcessHeap(), 0, (x))
#define WORKING_BUFFER_SIZE 15000
#define MAX_TRIES 3

#pragma warning( disable : 4244 )

//int NATUtils::GetLocalIPAddresses2(unsigned long Addresses[], int nMax)
//{
//    int nAddresses = 0;
//    DWORD dwSize = 0;
//    DWORD dwRetVal = 0;
//    unsigned int i = 0;
//
//    // Set the flags to pass to GetAdaptersAddresses
//    ULONG flags = GAA_FLAG_INCLUDE_PREFIX;
//
//    // default to unspecified address family (both)
//    //ULONG family = AF_UNSPEC;
//    ULONG family = AF_INET;         // IP V4
//    //ULONG family = AF_INET6;      // IP V6
//
//    LPVOID lpMsgBuf = NULL;
//    PIP_ADAPTER_ADDRESSES pAddresses = NULL;
//    ULONG outBufLen = 0;
//    ULONG Iterations = 0;
//    //char buff[100];
//    //DWORD bufflen=100;
//
//    PIP_ADAPTER_ADDRESSES pCurrAddresses = NULL;
//    PIP_ADAPTER_UNICAST_ADDRESS pUnicast = NULL;
//
//    outBufLen = WORKING_BUFFER_SIZE;
//    do {
//        pAddresses = (IP_ADAPTER_ADDRESSES *) MALLOC(outBufLen);
//        if (pAddresses == NULL)
//            return -1;
//        dwRetVal = GetAdaptersAddresses(family, flags, NULL, pAddresses, &outBufLen);
//        if (dwRetVal == ERROR_BUFFER_OVERFLOW) 
//        {
//            FREE(pAddresses);
//            pAddresses = NULL;
//        }
//        else 
//            break;
//        Iterations++;
//    } while ((dwRetVal == ERROR_BUFFER_OVERFLOW) && (Iterations < MAX_TRIES));
//
//    if (dwRetVal == NO_ERROR) 
//    {
//        pCurrAddresses = pAddresses;
//        while (pCurrAddresses) 
//        {
//            pUnicast = pCurrAddresses->FirstUnicastAddress;
//            if (pUnicast)
//            {
//                for (i = 0; pUnicast != NULL; i++)
//                {
//                    if (pUnicast->Address.lpSockaddr->sa_family == AF_INET)
//                    {
//                        sockaddr_in *sa_in = (sockaddr_in *)pUnicast->Address.lpSockaddr;
//                        //printf("\tIPV4:%s\n",inet_ntop(AF_INET,&(sa_in->sin_addr),buff,bufflen));
//
//                        // copy address out
//                        memcpy(&Addresses[nAddresses++], &(sa_in->sin_addr), sizeof(in_addr));
//                        if(nAddresses >= nMax)
//                            break;
//                    }
//                    else if (pUnicast->Address.lpSockaddr->sa_family == AF_INET6)
//                    {
//                        sockaddr_in6 *sa_in6 = (sockaddr_in6 *)pUnicast->Address.lpSockaddr;
//                        //printf("\tIPV6:%s\n",inet_ntop(AF_INET6,&(sa_in6->sin6_addr),buff,bufflen));
//                    }
//                    else
//                    {
//                        // unspecified
//                    }
//                    pUnicast = pUnicast->Next;
//                }
//            }
//            pCurrAddresses = pCurrAddresses->Next;
//        }
//    } 
//    else
//    {
//        // error
//        if (dwRetVal == ERROR_NO_DATA)
//        {
//            return -1; // No addresses were found for the requested parameters
//        }
//        else
//        {
//            if (FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, 
//                NULL, dwRetVal, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPTSTR) & lpMsgBuf, 0, NULL)) 
//             {
//                //printf("\tError: %s", lpMsgBuf);
//                LocalFree(lpMsgBuf);
//                if (pAddresses)
//                    FREE(pAddresses);
//                return -1;
//            }
//        }
//    }
//
//    if (pAddresses)
//        FREE(pAddresses);
//
//    return nAddresses;
//
//}
//
//int NATUtils::GetLocalIPAddresses(unsigned long Addresses[], int nMax)
//{
//    //InitializeWinsock();
//    // init windows sockets lib
//    static WSADATA WsaData;
//    if (WSAStartup(0x202, &WsaData) == SOCKET_ERROR)
//    {
//        int error = WSAGetLastError();
//        WSACleanup();
//        return error;
//    }
//
//    struct hostent *haddr;
//    char           szMyName[128];
//    unsigned long  NameLength = 128;
//    int            nAddresses;
//
//    LPTSTR cpName = new TCHAR [MAX_COMPUTERNAME_LENGTH+1];
//    LPDWORD lpnSize = new DWORD;
//    GetComputerName((cpName), lpnSize);
//    //GetComputerName(szMyName, &NameLength);
//    if (!(haddr=gethostbyname(szMyName)))
//    {
//        return -1;
//    }
//
//    for (nAddresses=0 ; nAddresses<nMax ; nAddresses++)
//    {
//        if (haddr->h_addr_list[nAddresses] == NULL)
//        {
//            break;
//        }
//
//        memcpy(&Addresses[nAddresses], haddr->h_addr_list[nAddresses], 4);
//    }
//
//    return nAddresses;
//}


EulerAngles Eul_(float ai, float aj, float ah, int order)
{
    EulerAngles ea;
    ea.x = ai; ea.y = aj; ea.z = ah;
    ea.w = order;
    return (ea);
}

/* Construct quaternion from Euler angles (in radians). */
Quat Eul_ToQuat(EulerAngles ea)
{
    Quat qu;
    double a[3], ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
    int i,j,k,h,n,s,f;
    EulGetOrd(ea.w,i,j,k,h,n,s,f);
    if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}
    if (n==EulParOdd) ea.y = -ea.y;
    ti = ea.x*0.5; tj = ea.y*0.5; th = ea.z*0.5;
    ci = cos(ti);  cj = cos(tj);  ch = cos(th);
    si = sin(ti);  sj = sin(tj);  sh = sin(th);
    cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
    if (s==EulRepYes) {
        a[i] = cj*(cs + sc);	/* Could speed up with */
        a[j] = sj*(cc + ss);	/* trig identities. */
        a[k] = sj*(cs - sc);
        qu.w = cj*(cc - ss);
    } else {
        a[i] = cj*sc - sj*cs;
        a[j] = cj*ss + sj*cc;
        a[k] = cj*cs - sj*sc;
        qu.w = cj*cc + sj*ss;
    }
    if (n==EulParOdd) a[j] = -a[j];
    qu.x = a[X]; qu.y = a[Y]; qu.z = a[Z];
    return (qu);
}

/* Construct matrix from Euler angles (in radians). */
void Eul_ToHMatrix(EulerAngles ea, HMatrix M)
{
    double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
    int i,j,k,h,n,s,f;
    EulGetOrd(ea.w,i,j,k,h,n,s,f);
    if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}
    if (n==EulParOdd) {ea.x = -ea.x; ea.y = -ea.y; ea.z = -ea.z;}
    ti = ea.x;	  tj = ea.y;	th = ea.z;
    ci = cos(ti); cj = cos(tj); ch = cos(th);
    si = sin(ti); sj = sin(tj); sh = sin(th);
    cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
    if (s==EulRepYes) {
        M[i][i] = cj;	  M[i][j] =  sj*si;    M[i][k] =  sj*ci;
        M[j][i] = sj*sh;  M[j][j] = -cj*ss+cc; M[j][k] = -cj*cs-sc;
        M[k][i] = -sj*ch; M[k][j] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
    } else {
        M[i][i] = cj*ch; M[i][j] = sj*sc-cs; M[i][k] = sj*cc+ss;
        M[j][i] = cj*sh; M[j][j] = sj*ss+cc; M[j][k] = sj*cs-sc;
        M[k][i] = -sj;	 M[k][j] = cj*si;    M[k][k] = cj*ci;
    }
    M[W][X]=M[W][Y]=M[W][Z]=M[X][W]=M[Y][W]=M[Z][W]=0.0; M[W][W]=1.0;
}

/* Convert matrix to Euler angles (in radians). */
EulerAngles Eul_FromHMatrix(HMatrix M, int order)
{
    EulerAngles ea;
    int i,j,k,h,n,s,f;
    EulGetOrd(order,i,j,k,h,n,s,f);
    if (s==EulRepYes) {
        double sy = sqrt(M[i][j]*M[i][j] + M[i][k]*M[i][k]);
        if (sy > 16*FLT_EPSILON) {
            ea.x = atan2((double)M[i][j], (double)M[i][k]);
            ea.y = atan2(sy, (double)M[i][i]);
            ea.z = atan2(M[j][i], -M[k][i]);
        } else {
            ea.x = atan2(-M[j][k], M[j][j]);
            ea.y = atan2(sy, (double)M[i][i]);
            ea.z = 0;
        }
    } else {
        double cy = sqrt(M[i][i]*M[i][i] + M[j][i]*M[j][i]);
        if (cy > 16*FLT_EPSILON) {
            ea.x = atan2(M[k][j], M[k][k]);
            ea.y = atan2((double)-M[k][i], cy);
            ea.z = atan2(M[j][i], M[i][i]);
        } else {
            ea.x = atan2(-M[j][k], M[j][j]);
            ea.y = atan2((double)-M[k][i], cy);
            ea.z = 0;
        }
    }
    if (n==EulParOdd) {ea.x = -ea.x; ea.y = - ea.y; ea.z = -ea.z;}
    if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}
    ea.w = order;
    return (ea);
}

/* Convert quaternion to Euler angles (in radians). */
EulerAngles Eul_FromQuat(Quat q, int order)
{
    HMatrix M;
    double Nq = q.x*q.x+q.y*q.y+q.z*q.z+q.w*q.w;
    double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
    double xs = q.x*s,	  ys = q.y*s,	 zs = q.z*s;
    double wx = q.w*xs,	  wy = q.w*ys,	 wz = q.w*zs;
    double xx = q.x*xs,	  xy = q.x*ys,	 xz = q.x*zs;
    double yy = q.y*ys,	  yz = q.y*zs,	 zz = q.z*zs;
    M[X][X] = 1.0 - (yy + zz); M[X][Y] = xy - wz; M[X][Z] = xz + wy;
    M[Y][X] = xy + wz; M[Y][Y] = 1.0 - (xx + zz); M[Y][Z] = yz - wx;
    M[Z][X] = xz - wy; M[Z][Y] = yz + wx; M[Z][Z] = 1.0 - (xx + yy);
    M[W][X]=M[W][Y]=M[W][Z]=M[X][W]=M[Y][W]=M[Z][W]=0.0; M[W][W]=1.0;
    return (Eul_FromHMatrix(M, order));
}
